/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.23
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *
 *                                                                         *
 ***************************************************************************/
#include "pathTest.hpp"
#include <iostream>
#include <fstream>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/trajectory.hpp>
#include <kdl/trajectory_composite.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/trajectory_stationary.hpp>
#include <sys/time.h>
#include <time.h>

#define IF_DEBUG false
#define IF_WRITE false
#define NSEC_PER_SEC    1000000000
#define TSET_NUM    10000

using namespace KDL;

PathTest::PathTest()
{
    this->puma = Puma560();

}

PathTest::~PathTest()
{

}

void PathTest::printTimeNano(struct timespec& t1,struct timespec& t2)
{
    if(t1.tv_nsec>t2.tv_nsec)
    {
        std::cout << t2.tv_sec-t1.tv_sec-1 << "second," <<
                     NSEC_PER_SEC-t1.tv_nsec+t2.tv_nsec << "nano second" << std::endl;
    }
    else
    {
        std::cout << t2.tv_sec-t1.tv_sec << "second," <<
                     t2.tv_nsec - t1.tv_nsec << "nano second" << std::endl;
    }
}

void PathTest::pathConfigure(Path* path,VelocityProfile* velProf)
{
    Trajectory* traj = new Trajectory_Segment(path, velProf);
//    Trajectory_Composite* ctraj = new Trajectory_Composite();
//    std::cout << "1" <<std::endl;
//    ctraj->Add(traj);
    double dt=traj->Duration()/10.0;
    std::ofstream ofp("./testData/trajectoryPos.dat");
    std::ofstream ofv("./testData/trajectoryVel.dat");
    std::ofstream ofa("./testData/trajectoryAcc.dat");
    for(double t=0.0; t<=traj->Duration(); t+=dt)
    {
        Frame currentPos;
        Twist currentVel;
        Twist currentAcc;
        currentPos = traj->Pos(t);
        currentVel = traj->Vel(t);
        currentAcc = traj->Acc(t);
        if (IF_WRITE)
        {
            for(int i=0; i<4; i++)
            {
                for(int j=0; j<4; j++)
                {
                    ofp << currentPos(i,j) << "\t";
                }
            }
            ofp << "\n";
            for(int i=0; i<6; i++)
            {
                ofv << currentVel(i) << "\t";
            }
            ofv <<"\n";
            for(int i=0; i<6; i++)
            {
                ofa << currentAcc(i) << "\t";
            }
            ofa <<"\n";
        }
    }
    ofp.close();
    ofv.close();
    ofa.close();
    delete traj;
    traj = NULL;

}

void PathTest::pathLineTest()
{
    Frame fs, fe;
    ChainFkSolverPos_recursive fkPosSolver=ChainFkSolverPos_recursive(this->puma);
    int jn = this->puma.getNrOfJoints();
    JntArray jointStart=JntArray(jn);
    JntArray jointEnd=JntArray(jn);
    struct timespec t1,t2;
    Path_Line* path = NULL;
    VelocityProfile* velProf = NULL;
    double tmp;
    for (int i=0; i<jn; i++)
    {
        random(jointStart(i));
        random(tmp);
        jointEnd(i)=jointStart(i)+0.1*tmp;
    }

    assert(fkPosSolver.JntToCart(jointStart, fs)==0);
    if(IF_DEBUG)
    {
        std::cout << "start Frame:" << std::endl;
        std::cout << fs <<std::endl;
    }

    assert(fkPosSolver.JntToCart(jointEnd, fe)==0);
    if(IF_DEBUG)
    {
        std::cout << "end Frame:" << std::endl;
        std::cout << fe <<std::endl;
    }

    std::cout << "LINE(trap velocity):" <<std::endl;
   clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++){
        path = new Path_Line(fs,fe,new RotationalInterpolation_SingleAxis(),0.1);//eqradius=0.1
        velProf = new VelocityProfile_Trap(0.2,0.2);
        velProf->SetProfile(0,path->PathLength());
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        this->pathConfigure(path,velProf);
//    delete path;
//    delete velProf;
//    path = NULL;
//    velProf = NULL;
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

    std::cout << "LINE(half trap velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++){
    path = new Path_Line(fs,fe,new RotationalInterpolation_SingleAxis(),0.1);//eqradius=0.1
    velProf = new VelocityProfile_TrapHalf(0.2,0.2);
    velProf->SetProfile(0, path->PathLength());
    if(IF_DEBUG)
    {
        std::cout << "pathLength:" << path->PathLength() <<std::endl;
        std::cout << "duration: " << velProf->Duration() <<std::endl;
    }
    this->pathConfigure(path,velProf);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

    VelocityProfile_Spline* velProfSpline = NULL;
    std::cout << "LINE(spline velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++){
        path = new Path_Line(fs,fe,new RotationalInterpolation_SingleAxis(),0.1);//eqradius=0.1
        velProfSpline= new VelocityProfile_Spline( );
        velProfSpline->SetProfileDuration(0.0, 0.0,path->PathLength(), 0.0, 0.5);
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProfSpline->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProfSpline);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

}

void PathTest::pathCircleTest()
{
    Frame fs, fe;
    Vector center;
    struct timespec t1,t2;
    Path_Circle* path =NULL;
    VelocityProfile* velProf = NULL;
    center = Vector(0,0,0);
    fs = Frame(Rotation::RPY(0.1,0.2,0.3),Vector(1,0,0));
    fe = Frame(Rotation::RPY(0.2,0.3,0.1),Vector(0,1,0));
    if(IF_DEBUG)
    {
        std::cout << "start Frame:" << std::endl;
        std::cout << fs <<std::endl;
        std::cout << "end Frame:" << std::endl;
        std::cout << fe <<std::endl;
    }
    VelocityProfile_Spline* velProfSpline = NULL;
    std::cout << "LINE(spline velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++)
    {
        path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(),0.1);
        velProfSpline= new VelocityProfile_Spline( );
        velProfSpline->SetProfileDuration(0.0, 0.0,path->PathLength(),0.0, 1.4);
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProfSpline->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProfSpline);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

    std::cout << "LINE(trap velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++)
    {
        path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(),0.1);
        velProf = new VelocityProfile_Trap(0.2,0.2);
        velProf->SetProfile(0,path->PathLength());
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProf);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

    std::cout << "LINE(half trap velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++)
    {
        path = new Path_Circle(fs, center, fe.p, fe.M, M_PI_2, new RotationalInterpolation_SingleAxis(),0.1);
        velProf = new VelocityProfile_TrapHalf(0.2,0.2);
        velProf->SetProfile(0, path->PathLength());
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProf);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

}

void PathTest::pathPointTest()
{
    Frame fs, fe;
    ChainFkSolverPos_recursive fkPosSolver=ChainFkSolverPos_recursive(this->puma);
    int jn = this->puma.getNrOfJoints();
    JntArray jointStart=JntArray(jn);
    JntArray jointEnd=JntArray(jn);
    struct timespec t1,t2;
    Path_RoundedComposite* path = NULL;
    VelocityProfile* velProf = NULL;
    double tmp;
    for (int i=0; i<jn; i++)
    {
        random(jointStart(i));
        random(tmp);
        jointEnd(i)=jointStart(i)+0.1*tmp;
    }

    assert(fkPosSolver.JntToCart(jointStart, fs)==0);
    if(IF_DEBUG)
    {
        std::cout << "start Frame:" << std::endl;
        std::cout << fs <<std::endl;
    }

    assert(fkPosSolver.JntToCart(jointEnd, fe)==0);
    if(IF_DEBUG)
    {
        std::cout << "end Frame:" << std::endl;
        std::cout << fe <<std::endl;
    }
    VelocityProfile_Spline* velProfSpline = NULL;
    std::cout << "LINE(spline velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++){
        path = new Path_RoundedComposite(0.2,0.1,new RotationalInterpolation_SingleAxis());
        path->Add(fs);
        path->Add(fe);
        path->Finish();
        velProfSpline= new VelocityProfile_Spline( );
        velProfSpline->SetProfileDuration(0.0, 0.0,path->PathLength(),0.0, 0.5);
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProfSpline->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProfSpline);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

    std::cout << "LINE(trap velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++){
        path = new Path_RoundedComposite(0.2,0.1,new RotationalInterpolation_SingleAxis());
        path->Add(fs);
        path->Add(fe);
        path->Finish();
        velProf = new VelocityProfile_Trap(0.2,0.2);
        velProf->SetProfile(0,path->PathLength());
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProf);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1, t2);

    std::cout << "LINE(half trap velocity):" <<std::endl;
    clock_gettime(CLOCK_REALTIME, &t1);
    for(int i=0; i<TSET_NUM; i++){
        path = new Path_RoundedComposite(0.2,0.1,new RotationalInterpolation_SingleAxis());
        path->Add(fs);
        path->Add(fe);
        path->Finish();
        velProf = new VelocityProfile_TrapHalf(0.2,0.2);
        velProf->SetProfile(0, path->PathLength());
        if(IF_DEBUG)
        {
            std::cout << "pathLength:" << path->PathLength() <<std::endl;
            std::cout << "duration: " << velProf->Duration() <<std::endl;
        }
        this->pathConfigure(path, velProf);
    }
    clock_gettime(CLOCK_REALTIME, &t2);
    printTimeNano(t1,t2);

}
